#ifndef _TRAJECTORYGENERATOR_H_
#define _TRAJECTORYGENERATOR_H_

#include "params.h"
#include "global.h"
#include <string>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

/*
    质心轨迹生成器
        角标命名规则：右上角开始顺时针命名
*/

class TrajectoryGenerator
{
private:
    int setflag_;       // 调用函数前先设置期望值
    int k_;             // 当前为第k个周期
    double block_para_; // 堵转保护系数

    double v_x_d_B_;     // 期望x速度(B系)
    double v_y_d_B_;     // 期望y速度(B系)
    double omega_z_d_B_; // 期望绕z角速度(B系)

    Vector3d p_0_com_d_O_; // 上一控制周期求得此刻期望位置
    Vector3d v_0_com_d_O_; // 上一控制周期求得此刻期望速度
    Vector3d p_0_com_O_;   // 此刻实际位置

    Vector3d v_k_com_d_O_; // 期望质心速度(2.21)
    Vector3d p_k_com_d_O_; // 期望质心位置(2.63)
    Vector3d omega_k_d_O_; // 期望角速度(2.65)
    Vector3d THETA_k_d_;   // 期望欧拉角(2.76)

    Vector<double, 13 * PREDICTION_HORIZON> D_;

public:
    TrajectoryGenerator();
    ~TrajectoryGenerator();

    void setDesired(const double &v_x, const double &v_y, const double &omega_z); 
    void calculateAll(const Matrix3d &Rot); 
    void getD(Vector<double, 13 * PREDICTION_HORIZON> &vectorForD);
    // Vector3d *getv_k_com_d_O(int k, Matrix3d *Rot);
    // Vector3d *getp_k_com_d_O(int k, Vector3d *p_0_com_O, Matrix3d *Rot);
    // Vector3d *getomega_k_d_O();
    // Vector3d *getTHETA_k_d();
};

#endif